/*
 * pid.c
 *
 *  Created on: Dec 10, 2023
 *      Author: 86152
 */

#ifndef PID_C_
#define PID_C_

#include "zf_common_headfile.h"

pid_param_t suspmotor_pid;  //悬浮电机pid（防止自旋）
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     给pid赋值
// 参数说明     pid_param_t *pid, float kp, float ki, float kd, float i_max, float low_pass
// 返回参数     无
// 使用示例     pid_adjust(&pid, 0.01, 0.02, 0.03, 50, 0.2);
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void pid_adjust(pid_param_t *pid, float kp, float ki, float kd, float i_max, float low_pass)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->i_max = i_max;
    pid->low_pass = low_pass;
}

void pid_pd_adjust(pid_param_t *pid, float kp, float kd)
{
    pid->kp = kp;
    pid->kd = kd;
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     位置式pid
// 参数说明     pid_param_t类型结构体 error(偏差)
// 返回参数     无
// 使用示例     pid_solve(&pid, error);
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
float pid_solve(pid_param_t *pid, float error)
{
    //这里的微分项输出使用到了低通滤波算法
    pid->out_d = (error - pid->out_p) * pid->low_pass + pid->out_d * (1 - pid->low_pass);
    pid->out_p = error;
    pid->out_i += error;

    if (pid->ki != 0) pid->out_i = MINMAX(pid->out_i, -pid->i_max / pid->ki, pid->i_max / pid->ki);

    return pid->kp * pid->out_p + pid->ki * pid->out_i + pid->kd * pid->out_d;
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     PD_PID
// 参数说明     pid_param_t类型结构体 error(偏差)
// 返回参数     无
// 使用示例     pid_pd(&pid, error);
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
float pid_pd(pid_param_t *pid, float error)
{
    pid->out_p = error;
    pid->out_d = error - pid->pre_error;
    pid->pre_error = error;

    return pid->kp * pid->out_p + pid->kd * pid->out_d;
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     增量式pid
// 参数说明     pid_param_t类型结构体 error(偏差)
// 返回参数     解算值(float)
// 使用示例     pid_solve(&pid, error);
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
float increment_pid_solve(pid_param_t *pid, float error)
{
    pid->out_d = MINMAX(pid->kd * (error - 2 * pid->pre_error + pid->pre_pre_error), -pid->d_max, pid->d_max);
    pid->out_p = MINMAX(pid->kp * (error - pid->pre_error), -pid->p_max, pid->p_max);
    pid->out_i = MINMAX(pid->ki * error, -pid->i_max, pid->i_max);

    pid->pre_pre_error = pid->pre_error;
    pid->pre_error = error;

    return pid->out_p + pid->out_i + pid->out_d;
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     增量式pd
// 参数说明     pid_param_t类型结构体 error(偏差)
// 返回参数     解算值(float)
// 使用示例     pid_solve(&pid, error);
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
float increment_pd(pid_param_t *pid, float error)
{
    pid->out_d = pid->kd * (error - 2 * pid->pre_error + pid->pre_pre_error);
    pid->out_p = pid->kp * (error - pid->pre_error);

    pid->pre_pre_error = pid->pre_error;
    pid->pre_error = error;

    return pid->out_p + pid->out_d;
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     变积分pid，
// 参数说明     pid_param_t类型结构体 error(偏差)
// 返回参数     解算值(float)
// 使用示例     pid_solve(&pid, error);
// 备注信息     e大i小
//-------------------------------------------------------------------------------------------------------------------

float change_kib = 4;

float changable_pid_solve(pid_param_t *pid, float error)
{
    pid->out_d = pid->kd * (error - 2 * pid->pre_error + pid->pre_pre_error);
    pid->out_p = pid->kp * (error - pid->pre_error);
    float ki_index = pid->ki;
    // 如果当前误差和前一次误差的和大于0，则进行变积分控制
    if (error + pid->pre_error > 0) {
        ki_index = (pid->ki) - (pid->ki) / (1 + exp(change_kib - 0.2 * fabs(error)));     // 变积分控制
    }

    pid->out_i = ki_index * error;
    pid->pre_pre_error = pid->pre_error;
    pid->pre_error = error;

    // 返回经过限制后的PID控制输出值，限制在p_max，-p_max，i_max，-i_max，d_max，-d_max之间
    return MINMAX(pid->out_p, -pid->p_max, pid->p_max)
           + MINMAX(pid->out_i, -pid->i_max, pid->i_max)
           + MINMAX(pid->out_d, -pid->d_max, pid->d_max);
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     Bang-Bang控制pid
// 参数说明     pid_param_t类型结构体 error(偏差)
// 返回参数     解算值(float)
// 使用示例     pid_solve(&pid, error);
// 备注信息     这段代码是一个 Bang-Bang 控制与 PID 控制相结合的算法。当误差超过一定阈值时，它采用 Bang-Bang 控制；当误差在一
//             定范围内时，它切换到 PID 控制
//-------------------------------------------------------------------------------------------------------------------
float bangbang_out = 0;

float bangbang_pid_solve(pid_param_t *pid, float error) {
    float BangBang_output = 15000, BangBang_error = 8;
    pid->error = error;

    //BangBang
    if (error > BangBang_error || error < -BangBang_error) {
        bangbang_out = (error > 0) ? BangBang_output : (-BangBang_output);
    } else {
        pid->out_d = pid->kd * (error - 2 * pid->pre_error + pid->pre_pre_error);

        pid->out_p = pid->kp * (error - pid->pre_error);

        pid->out_i = pid->ki * error;

        bangbang_out = MINMAX(pid->out_p, -pid->p_max, pid->p_max)
                       + MINMAX(pid->out_i, -pid->i_max, pid->i_max)
                       + MINMAX(pid->out_d, -pid->d_max, pid->d_max);

    }
    pid->pre_pre_error = pid->pre_error;
    pid->pre_error = error;

    return bangbang_out;
}



#endif /* PID_C_ */
